/*
 * MMA7455.cpp
 *
 * Created: 09.05.2012 12:51:27
 *  Author: Maxim Karban <maxim.karban@gmail.com>
 */ 

#include <stdint.h>
#include "mma7455.h"

uint8_t mmabuf[6] = {0, 0, 0, 0, 0, 0};

void MMADecode(int16_t * accel_x, int16_t * accel_y, int16_t * accel_z){
  *accel_x = ((mmabuf[1] << 8) | (mmabuf[0])) << 6;
  *accel_x >>= 2;
  *accel_y = ((mmabuf[3] << 8) | (mmabuf[2])) << 6;
  *accel_y >>= 2;
  *accel_z = ((mmabuf[5] << 8) | (mmabuf[4])) << 6;
  *accel_z >>= 2;
}

int32_t accel_x_prev = 0;
int32_t accel_y_prev = 0;
int32_t accel_z_prev = 0;


void MMAFilter(int16_t * accel_x, int16_t * accel_y, int16_t * accel_z){
  #define kf 128
  *accel_x = ((int32_t)(accel_x_prev*(1024-kf)+(int32_t)(*accel_x)*kf))>>10;
  accel_x_prev = *accel_x;
  *accel_y = ((int32_t)(accel_y_prev*(1024-kf)+(int32_t)(*accel_y)*kf))>>10;
  accel_y_prev = *accel_y;
  *accel_z = ((int32_t)(accel_z_prev*(1024-kf)+(int32_t)(*accel_z)*kf))>>10;
  accel_z_prev = *accel_z;
}